#ifndef HITSZ_DIP_PRACTICUM_PLANNER_H
#define HITSZ_DIP_PRACTICUM_PLANNER_H

#include <vector>
#include <Eigen/Dense>
#include "KMF.h"
#include <geometry_msgs/Twist.h>
#include "param.h"
namespace dip
{ 
    //全局路径规划状态
    enum class CrossStatus{
        START = 0,CROSSING = 1,CROSSED =2
    };

    //切换状态时的状态 
    enum class SwitchStatus{
        NONE = 0,START = 1,OVER = 2
    };

    //Astern(课设第三个任务)任务状态,主要是记录当前任务
    enum class AsternTask{
        STOP=0,SWITCH = 1,
        CROSS = 2,XJBZ = 3
    };
    //0是安全模式，停止;1为任务切换暂态,会重置一些量
    //2为要求的穿过圆柱;XJBZ是音译，就是瞎动测试定位
    inline AsternTask int2task(uint8_t x)
    {
        switch (x)
        {
        case 0:{return AsternTask::STOP;}
        case 1:{return AsternTask::SWITCH;}
        case 2:{return AsternTask::CROSS;}
        case 3:{return AsternTask::XJBZ;}
        default:{return AsternTask::STOP;}
        }
    }
    struct PDController{
        float Kp;
        float Kd;

        float ref;
        float feedback;
        float u;//控制量输出
        float u_max;//最大输出

        float last_error;
        float error;
        float derror;

        PDController()
        {
            Kp = Kd = 0;
            ref = 0;
            feedback = 0;
            u = 0;
            u_max = 0;
            last_error = 0;
            error = 0;
            derror = 0;
        }

        float calc(float _ref,float _feedback)
        {
            ref = _ref;
            feedback = _feedback;
            last_error = error;
            error = ref-feedback;
            derror = error - last_error;
            u = Kp*error + Kd*derror;
            if (u>u_max){u = u_max;}
            else if (u<-u_max){u = -u_max;}
            return u;
        }
    };
    class LocalPlanner
    {//局部路径规划生成/cmd_vel的所需msg
    public:
        LocalPlanner()
        {
            controller.Kp = 1;
            controller.Kd = 0.1;
            controller.u_max = 0.2;
            vx = 0.1;
        }

        void update(const std::vector<Eigen::Vector3f> &_trj,const Eigen::Vector3f &_pose)
        {
            if (_trj.size()==0)
            { //如果停止
                std::cout<<"[LOCAL PLANNER]:STOP"<<std::endl;
                command.linear.x = 0;
		        command.linear.y = 0;
		        command.linear.z = 0;
		        command.angular.x = 0;
		        command.angular.y = 0;
		        command.angular.z = 0;
                return ;
            }
            for (int i=0;i<_trj.size();i++)
            {
                std::cout<<"[TRJ] "<<i<<":"<<_trj[i].transpose()<<std::endl;
            }
            // float distance = pow((_pose(0,0) - _trj[0](0,0)),2) + pow((_pose(1,0) - _trj[0](1,0)),2);
             float distance =pow((_pose(1,0) - _trj[0](1,0)),2);
            if (distance < 0.06)
            {//如果已经到达下一个点附近
                if (fabs(_pose(2,0) - _trj[0](2,0))>0.07)
                { //如果转向没有完成
                    controller.calc(_trj[0](2,0),_pose(2,0));
                    command.angular.z = -1*controller.u;
                    command.linear.x = 0;
		            command.linear.y = 0;
		            command.linear.z = 0;
		            command.angular.x = 0;
		            command.angular.y = 0;
                }
                else
                { //如果转向完成
                    command.linear.x = 0;
		            command.linear.y = 0;
		            command.linear.z = 0;
		            command.angular.x = 0;
		            command.angular.y = 0;
		            command.angular.z = 0;
                }
            }
            else
            {
                command.linear.x = vx;
		        command.linear.y = 0;
		        command.linear.z = 0;
		        command.angular.x = 0;
		        command.angular.y = 0;
		        command.angular.z = 0;
            }
        }

    public:
        float vx;//恒定x方向速度
        geometry_msgs::Twist command;//指令
        PDController controller;
    };

    class GlobalPlanner
    {//全局路径规划,只做了一个状态机
    public:
        GlobalPlanner()
        {
            this->task = AsternTask::STOP;
            this->cross_status = CrossStatus::START;
            this->switch_status = SwitchStatus::NONE;
            this->distance_thre = 0.06;//要求误差在20cm
            this->switch_pose<<(Param::LEFT_COLUMN_COORDS(0,0)+Param::RIGHT_COLUMN_COORDS(0,0))/2,-1,1.57;
            //默认切换点为(p_lx + p_rx / 2,-1.5,90deg),如果在对侧会变为(p_lx + p_rx / 2,1.5,-90deg)
        };

        /**
         * @brief 切换任务，对当前任务进行更新;
         * 如果和当前任务不同会先切入switch模式,但是需要一直输入
         */ 
        void switch_task_status(AsternTask _task)
        {
            if (this->task == _task)
            { //如果和当前一致
                switch(_task)
                {
                    case (AsternTask::STOP):{this->task_stop();return;}
                    case (AsternTask::SWITCH):{this->task_switch();return;}
                    case (AsternTask::CROSS):{this->task_cross();return;}
                    case (AsternTask::XJBZ):{this->task_xjbz();return;}
                }
            }
            else
            {//如果不一致,进入切换状态
                if (_task==AsternTask::STOP)
                {
                    this->task = _task;
                    std::cout<<"[Global Planner]:STOP"<<std::endl;
                    return ;
                }
                if (this->task == AsternTask::SWITCH && this->switch_status == SwitchStatus::OVER)
                { //如果切换完成,将task置为目标状态
                    this->cross_status == CrossStatus::START;
                    this->task = _task;
                    std::cout<<"[Global Planner]:switch to task "<<2<<std::endl;
                }
                else
                { //切换
                    this->task = AsternTask::SWITCH;
                    this->switch_status = SwitchStatus::START;
                    this->task_switch();
                    std::cout<<"[Global Planner]:switch to task switch"<<std::endl;
                }

            }

        }

        /**
         * @brief 根据当前状态和是否观测进行任务内部状态更新
         * 
         */ 
        void update(bool _observe_flag,const Eigen::Vector3f &_pose)
        {
            this->current_pose = _pose;
            if (! _observe_flag)
            { //如果没有观测到
                if (this->task==AsternTask::CROSS)
                { //如果在穿越前没有观测，切换为穿越
                    if (this->cross_status == CrossStatus::START)
                    {
                        this->cross_status = CrossStatus::CROSSING;
                    }
                    else if(this->cross_status == CrossStatus::CROSSING)
                    { //如果是正在穿越
                        float distance;
                        if (this->current_pose(2,0) < 3)
                        { //如果在对侧
                            // distance = pow((_pose(0,0) - switch_pose(0,0)),2) + pow((_pose(1,0) + switch_pose(1,0)),2);
                            distance =  pow((_pose(1,0) + switch_pose(1,0)),2);
                        }
                        else
                        {
                            // distance = pow((_pose(0,0) - switch_pose(0,0)),2) + pow((_pose(1,0) - switch_pose(1,0)),2);
                            distance = pow((_pose(1,0) - switch_pose(1,0)),2);
                        }

                        if (distance < this->distance_thre  )
                        { //如果到达了切换位置,那么任务结束
                            this->cross_status = CrossStatus::CROSSED;
                        }
                    }
                }
            }
            else
            {//如果观测到
                if (this->task == AsternTask::SWITCH)
                { //如果是switch状态,直到到达切换目标点结束切换
                    float distance;
                    bool angle_distance = false;
                    if (_pose(1,0)<0)
                    { //如果在对侧
                        distance = pow((_pose(0,0) - switch_pose(0,0)),2) + pow((_pose(1,0) - switch_pose(1,0)),2);
                        if (fabs(_pose(2,0) - 1.57)<0.2){angle_distance = true;}
                    }
                    else
                    {
                        distance = pow((_pose(0,0) - switch_pose(0,0)),2) + pow((_pose(1,0) + switch_pose(1,0)),2);
                        if (fabs(_pose(2,0) - 4.71)<0.2){angle_distance = true;}
                    }
                    if (distance < this->distance_thre && angle_distance)
                    {
                        this->switch_status = SwitchStatus::OVER;
                    }
                }
                // else if (this->task == AsternTask::CROSS)
                // {//如果是任务状态
                    // 
                // }

            }
        }

        /**
         * @brief 停止状态时轨迹生成,清空
         */ 
        void task_stop()
        {
            pose_trj.clear();
        }

        /**
         * @brief 切换状态轨迹生成
         */ 
        void task_switch()
        {
            pose_trj.clear();

            float dx = this->switch_pose(0,0) - this->current_pose(0,0);
            float dy = this->switch_pose(1,0) - this->current_pose(1,0);
            double theta = atan2(dy,dx);//dy为负值，得到theta<0
            if (theta<0){theta +=2*3.14;}

            if (fabs(this->current_pose(2,0) - theta)>0.07)
            { //如果转向没有完成
                Eigen::Vector3f rotate_pose;
                rotate_pose<<this->current_pose(0,0),this->current_pose(1,0),theta;
                pose_trj.push_back(rotate_pose);
            }

            //下一个目标点为(x,y,theta)
            if (this->current_pose(1,0) < 0)
            { //如果y<0
                Eigen::Vector3f middle_pose;
                middle_pose<<this->switch_pose(0,0),this->switch_pose(1,0),theta;
                float distance = pow((current_pose(0,0) - middle_pose(0,0)),2) + pow((current_pose(1,0) - middle_pose(1,0)),2);
                if (distance > this->distance_thre){pose_trj.push_back(middle_pose);}
                
            }
            else
            {
                Eigen::Vector3f middle_pose;
                middle_pose<<this->switch_pose(0,0),-this->switch_pose(1,0),theta;
                float distance = pow((current_pose(0,0) - middle_pose(0,0)),2) + pow((current_pose(1,0) - middle_pose(1,0)),2);
                std::cout<<"distance"<<distance<<std::endl;
                if (distance > this->distance_thre){pose_trj.push_back(middle_pose);}
            }

            //最后为切换坐标点
            if (this->current_pose(1,0) < 0)
            { //如果y<0
                pose_trj.push_back(this->switch_pose);
            }
            else
            {
                Eigen::Vector3f end_pose;
                end_pose<<this->switch_pose(0,0),-this->switch_pose(1,0),4.71;
                pose_trj.push_back(end_pose);
            }
        }

        void task_xjbz()
        {
            pose_trj.clear();
        }

        void task_cross()
        {
            pose_trj.clear();

            if (this->cross_status == CrossStatus::START)
            {//如果刚开始穿越，根据current_pose设定目标点
                std::cout<<"[Global Planner]:task cross,status start"<<std::endl;
                if (this->current_pose(1,0) < 0)
                { //如果y<0
                    Eigen::Vector3f middle_pose;
                    middle_pose<<this->switch_pose(0,0),0,1.57;
                    pose_trj.push_back(middle_pose);
                    Eigen::Vector3f end_pose;
                    end_pose<<this->switch_pose(0,0),-this->switch_pose(1,0),1.57;
                    pose_trj.push_back(end_pose);
                }
                else
                {
                    Eigen::Vector3f middle_pose;
                    middle_pose<<this->switch_pose(0,0),0,4.71;
                    pose_trj.push_back(middle_pose);
                    Eigen::Vector3f end_pose;
                    end_pose<<this->switch_pose(0,0),this->switch_pose(1,0),4.71;
                    pose_trj.push_back(end_pose);
                }
            }
            else if (this->cross_status == CrossStatus::CROSSING)
            {//如果正在穿越,根据当前theta生成关键点
                std::cout<<"[Global Planner]:task cross,status crossing"<<std::endl;
                if (this->current_pose(2,0) < 3)
                { //如果正行驶
                    Eigen::Vector3f end_pose;
                    end_pose<<this->switch_pose(0,0),-this->switch_pose(1,0),1.57;
                    pose_trj.push_back(end_pose);
                }
                else
                { //如果反向行驶
                    Eigen::Vector3f end_pose;
                    end_pose<<this->switch_pose(0,0),this->switch_pose(1,0),4.71;
                    pose_trj.push_back(end_pose);
                }
                std::cout<<"crossing:"<<pose_trj.size()<<std::endl;
            }
            else
            {//如果已经穿越完成,无任何指令，停止即可
                std::cout<<"[Global Planner]:task cross,status end"<<std::endl;
                return;
            }
        }

    public:
        CrossStatus cross_status;
        SwitchStatus switch_status;
        AsternTask task;

        std::vector<Eigen::Vector3f> pose_trj;
        Eigen::Vector3f current_pose;
        Eigen::Vector3f switch_pose;//切换状态时的阈值中心位姿
        float distance_thre;
        bool observe_flag;//是否观测到


    };
}
#endif